STEAP: Towards Online Estimation and Replanning

In this work, we present simultaneous trajectory estimation and planning (STEAP) a unified approach to solving continuous-time trajectory estimation and planning problems. Although, these problems are usually considered separately, within our framework we show how estimation and planning can benefit from each other and remove redundancy during computation. Each time-step the robot is tasked with finding the full continuous-time trajectory from start to goal, such that the history of the trajectory signifies the solution to the estimation problem, while the future of the trajectory signifies a solution to the planning problem. Building on recent work we employ incremental inference on probabilistic graphical models to solve this problem, and provide an approach that can contend with high-degree-of-freedom (DOF) trajectory space, uncertainty due to limited sensing capabilities, model inaccuracy, the stochastic effect of executing actions and can find a solution in real time. We evaluate our approach empirically on a mobile manipulator.